{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# OrientedPlane3 Factors"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "This header defines factors for working with planar landmarks represented by `gtsam.OrientedPlane3`.\n",
        "`OrientedPlane3` represents a plane using normalized coefficients $(n_x, n_y, n_z, d)$, where $(n_x, n_y, n_z)$ is the unit normal vector and $d$ is the distance from the origin along the normal.\n",
        "\n",
        "Factors defined:\n",
        "*   `OrientedPlane3Factor`: A binary factor connecting a `Pose3` and an `OrientedPlane3`. It measures the difference between the plane parameters as observed from the given pose and a measured plane.\n",
        "*   `OrientedPlane3DirectionPrior`: A unary factor on an `OrientedPlane3`. It penalizes the difference between the plane's normal direction and a measured direction (represented by the normal of a measured `OrientedPlane3`). **Note:** The factor error is 3D, but only constrains 2 degrees of freedom (direction). Consider using a more specific direction factor if only direction is measured."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/OrientedPlane3Factor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n",
        "from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n",
        "from gtsam.symbol_shorthand import X, P"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_header_md"
      },
      "source": [
        "## 1. `OrientedPlane3Factor`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_desc_md"
      },
      "source": [
        "Connects a `Pose3` (camera/robot pose) and an `OrientedPlane3` (landmark). The measurement is the plane as observed *from the sensor frame*.\n",
        "The error is calculated by transforming the global plane landmark into the sensor frame defined by the pose, and then computing the difference (`localCoordinates`) with the measured plane."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "factor_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "OrientedPlane3Factor: \n",
            "OrientedPlane3Factor Factor (x0, p0)\n",
            "Measured Plane : 0 0 1 1\n",
            "isotropic dim=3 sigma=0.05\n",
            "\n",
            "Error at ground truth: 0.0\n",
            "\n",
            "Error with noisy plane: 0.6469041114912286\n"
          ]
        }
      ],
      "source": [
        "# Ground truth plane (e.g., z=1 in world frame)\n",
        "gt_plane_world = OrientedPlane3(0, 0, 1, 1)\n",
        "\n",
        "# Ground truth pose\n",
        "gt_pose = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0))\n",
        "\n",
        "# Measurement: transform the world plane into the camera frame\n",
        "# measured_plane = gt_plane_world.transform(gt_pose)\n",
        "# C++ header: Plane measurement z is a 4D vector [a,b,c,d] coefficients:\n",
        "measured_plane_coeffs = gt_plane_world.planeCoefficients()\n",
        "plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)\n",
        "\n",
        "pose_key = X(0)\n",
        "plane_key = P(0)\n",
        "\n",
        "plane_factor = OrientedPlane3Factor(measured_plane_coeffs, plane_noise, pose_key, plane_key)\n",
        "plane_factor.print(\"OrientedPlane3Factor: \")\n",
        "\n",
        "# Evaluate error\n",
        "values = Values()\n",
        "values.insert(pose_key, gt_pose)\n",
        "values.insert(plane_key, gt_plane_world)\n",
        "error1 = plane_factor.error(values)\n",
        "print(f\"\\nError at ground truth: {error1}\")\n",
        "\n",
        "# Evaluate with slightly different plane estimate\n",
        "noisy_plane = OrientedPlane3(0.01, 0.01, 0.99, 1.05)\n",
        "values.update(plane_key, noisy_plane)\n",
        "error2 = plane_factor.error(values)\n",
        "print(f\"\\nError with noisy plane: {error2}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "prior_header_md"
      },
      "source": [
        "## 2. `OrientedPlane3DirectionPrior`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "prior_desc_md"
      },
      "source": [
        "A unary factor that puts a prior on the *direction* (normal vector) of an `OrientedPlane3`. The distance component (`d`) of the measured plane is ignored.\n",
        "The error is the difference between the estimated plane's normal and the measured plane's normal, but as directions only have 2 DOF, the noise model also has to have dimension 2."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "prior_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "OrientedPlane3DirectionPrior: \n",
            "OrientedPlane3DirectionPrior: Prior Factor on p0\n",
            "Measured Plane : 0 0 1 0\n",
            "isotropic dim=2 sigma=0.02\n",
            "\n",
            "Error for prior on noisy_plane: 0.2550239722533919\n",
            "Error for prior on gt_plane_world: 0.0\n"
          ]
        }
      ],
      "source": [
        "# Measured direction prior (e.g., plane normal is close to world Z axis)\n",
        "measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n",
        "direction_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.02)\n",
        "\n",
        "prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), direction_noise)\n",
        "prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n",
        "\n",
        "# Evaluate error using the 'noisy_plane' from the previous step\n",
        "error_prior = prior_factor.error(values) # values still contains plane_key -> noisy_plane\n",
        "print(f\"\\nError for prior on noisy_plane: {error_prior}\")\n",
        "\n",
        "# Evaluate error for ground truth plane\n",
        "values.update(plane_key, gt_plane_world)\n",
        "error_prior_gt = prior_factor.error(values)\n",
        "print(f\"Error for prior on gt_plane_world: {error_prior_gt}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
